package com.gunncs.actoriface;
import com.gunncs.actoriface.*;
package com.gunncs.actoriface;
import com.gunncs.actoriface.*;
import java.util.*;

public class DecisionActor implements Actor {
  public static final int TOLERANCE=5;
  public static final int DISTTOLERANCE=7;
  public static final int MAGICNUMBER=99;
  //States
  static final int IDLE = 0;  //Incurs handoff to simpleNav
  static final int OPEN = 1; //Yet to see end of opening from subsequent.get(1)
  static final int CLOSE = 2;  //Is there anything beyond subsiding?
  static final int DRIVE = 3; //GOTO selected action
  static final int UNDO = 4;

  public static final int HISTORYSIZE=100;
  int state;
  int nogodir;
  
  boolean shortpath;
  //Hysteresis data collection arrays of pairs
  double[][] lfHistory = new double[HISTORYSIZE][2];	//LEFT FRONT
  double[][] rfHistory = new double[HISTORYSIZE][2];	//RIGHT FRONT
  
  //Auxiliary data
  double[][] lbHistory = new double[HISTORYSIZE][2];  //LEFT BACK
  double[][] rbHistory = new double[HISTORYSIZE][2];  //RIGHT BACK
  
  Decision chosen;
  ShortPathActor shure;
  int closestep;
  //Decision tree structure
  Decision root;
  //ptr to current node
  Decision current;
  
  int direction;
  
  public DecisionActor() {
    direction=Decision.NORTH;
    root = new Decision(Decision.NORTH, Decision.NORTH);
    current = root;
    state = IDLE;
    chosen=null;
    closestep=1;
    nogodir=-99;
    shortpath= false;
    shure = new ShortPathActor(root.getPrior());
  }
  public String getMaze() {
    return "Example";
  }    
  

  public void act(Robot r){
    if(shortpath){
      shure.act(r);
    }else{
    switch(state){
      case IDLE:
        //handoff to lisa
        Simulator.out.println(state);
        return;
      case OPEN:
        //Drive waiting for subside
        r.goForward();
        r.waitDistance(1);
        Simulator.out.println(state);
      break;
      case CLOSE:
        //Check for undershoot & other factors
        if(closestep<2){
          r.goForward();
          r.waitDistance(6);
          closestep++;
	}
	else
	  closestep=0;
        //r.getServo().rotate(45.0);
        Simulator.out.println(state);
      break;
      case DRIVE:
        //Align and make turn
        int distance = chosen.getDistance();
	
	if(distance>0){
	  r.goForward();
	  r.waitDistance(20);
	}
	if(rotateDirection(chosen.getDirection(), false) == direction){
	  r.turnRight();
	  r.waitAngle(90);
	  r.goForward();
          r.waitDistance(7);
	}else if(rotateDirection(chosen.getDirection(), true) == direction){
	  r.turnLeft();
	  r.waitAngle(90);
	  r.goForward();
          r.waitDistance(7);
	}
	  nogodir=-99;
          state = IDLE;
        //Handoff to lisa
        Simulator.out.println(state);
        return;
      case UNDO:
        r.turnRight();
        r.waitAngle(180);
        state = IDLE;
        Simulator.out.println(state);
        break;
        
      default:
         Simulator.out.println(state);
        //Throw invalid state exception
        state = IDLE;
        //dump history
        //clear history
        lfHistory = new double[HISTORYSIZE][2];	//LEFT FRONT
  	rfHistory = new double[HISTORYSIZE][2];	//RIGHT FRONT
 	lbHistory = new double[HISTORYSIZE][2];  //LEFT BACK
  	rbHistory = new double[HISTORYSIZE][2];  //RIGHT BACK
  	break;
      }
    }
  }
 

  private void decide() {
    if(current.noSubsequent()){
      current.remove();
      if (!(current.noSubsequent())) {
        current.getPrior().peek().removeChoice(current);
      }
      current = current.getPrior().peek();
      nogodir=current.getCameFrom();
      state=UNDO;
    }else{ 
      ArrayList<Decision> choices = current.getChoices();
      Decision d=null;   
      for(int i=0; i<choices.size(); i++){
        d = choices.get(i);
        if(!d.getFail()){
          if(nogodir != d.getOptionDir()) break;
        }else d = null;
      } 
      chosen=d;
      if(chosen==null){
        for(int i=0; i<choices.size(); i++){
          d = choices.get(i);
          if(!d.getFail()){
            break;
          }else d = null;
        }
      }
      chosen.select();// choice has been taken
      // use decision d
    }
  }
  public void updateSensors(Sensor[] sensors) {
    //System.out.println("updating sensor "); 
    //remove last value 
    lfHistory[HISTORYSIZE-1][0] = -1.0;
    shift(lfHistory);
    rfHistory[HISTORYSIZE-1][0]=-1.0;
    shift(rfHistory);
    lbHistory[HISTORYSIZE-1][0]=-1.0;
    shift(lbHistory);
    rbHistory[HISTORYSIZE-1][0]=-1.0;
    shift(rbHistory);
    //insert new value
    lfHistory[0] = sensors[1].getState();
    rfHistory[0] = sensors[1].getState();
    lbHistory[0] = sensors[1].getState();
    rbHistory[0] = sensors[1].getState();
    if(shortpath)
      shure.updateSensors(sensors);
    else if(SimpleNavigator.adjusting){
    }
    else
      search();
  }
  
  private void shift(double[][] history){
    for(int j=history.length-1; j>0; j--){
      history[j]=history[j-1];
    }
  }
  
  
  private void search(){   //TREE Builder
    int priorstate=state;
    //System.out.println(lfHistory);
    if(state!=DRIVE)
     state = (RangeFinderAnalysis.calcCurrentState(lfHistory, rfHistory, lbHistory, rbHistory));
    double[][] flset = {lfHistory[lfHistory.length-1], 
     		lfHistory[lfHistory.length-2], lfHistory[lfHistory.length-3]};
     double[][] frset = {rfHistory[rfHistory.length-1], 
     		rfHistory[rfHistory.length-2], rfHistory[rfHistory.length-3]};
    switch(state){
      case IDLE:
        break;
      case OPEN:
        if(state != priorstate){
          //Determine side of opening
          if(RangeFinderAnalysis.risingOrFalling(true, flset)){
             current.addChoice(new Decision(rotateDirection(direction, false) ,direction));
          }
          if(RangeFinderAnalysis.risingOrFalling(true, frset)){
            current.addChoice(new Decision(rotateDirection(direction, true), direction));
          }
          
        }
        break;
      case CLOSE:
        if(RangeFinderAnalysis.plateau(flset) || RangeFinderAnalysis.plateau(frset) 
        				&& closestep>1)
          current.addChoice(new Decision(direction, direction));
        if(current.subsequent.size()>3 && closestep>1){
          if(flset[0][0]-frset[0][0] > DISTTOLERANCE ){
            current.subsequent.get(0).setDistance(99);
          }else if(flset[0][0] - frset[0][0] < -1 * DISTTOLERANCE){
            current.subsequent.get(1).setDistance(99);
	  }
	  state=DRIVE;  
        }
        
        break;
        case DRIVE:
          if(current.subsequent.size()>1)
            decide();
          else 
            state=IDLE;
        break;
    }
  }

  
  
  public static int rotateDirection(int direction, boolean clockwise) {
    if(clockwise){
      switch(direction){
        case Decision.NORTH:
          return Decision.EAST;
        case Decision.SOUTH:
          return Decision.WEST;
        case Decision.EAST:
          return Decision.SOUTH;
        case Decision.WEST:
          return Decision.NORTH;
        default:
          return 99;
      }
    }else {
      switch(direction){
        case Decision.SOUTH:
          return Decision.EAST;
        case Decision.NORTH:
          return Decision.WEST;
        case Decision.EAST:
          return Decision.NORTH;
        case Decision.WEST:
          return Decision.SOUTH;
        default:
          return 99;
      }
    }
  }
}